#ifndef PROBE_H
#define PROBE_H

#include <Arduino.h>
#include "MLX90640_API.h"
#include "MLX90640_I2C_Driver.h"
#include "kalman_filter.h"
#include "../shared_val.h"

#define MXL_STARTUP_DELAY 3700

#define TA_SHIFT 8 //Default shift for MLX90640 in open air
#define MLX_VDD  11
#define MLX_SDA  12
#define MLX_SCL  13
#define KALMAN


#if defined(KALMAN)
#include "kalman_filter.h"
const static float init_P = 0.1;
const static float init_G = 0.0;
const static float init_O = 26;

static KFPTypeS kfpVar3Array[768];  // 卡尔曼滤波器变量数组
// 初始化卡尔曼滤波器数组的函数

void KalmanArrayInit() {
    // 循环遍历数组中的每个元素
    for (int i = 0; i < 768; ++i) {
        // 初始化每个元素
        kfpVar3Array[i] = (KFPTypeS){
         init_P,     //估算协方差. 初始化值为 0.02
         init_G,     //卡尔曼增益. 初始化值为 0
         init_O    //卡尔曼滤波器输出. 初始化值为 0
        };
    }
}
#endif

const byte MLX90640_address = 0x33;                          
float T_center;  
int max_x, max_y, min_x, min_y;
bool mlx_is_connected = false;
int R_colour, G_colour, B_colour; 
static float mlx90640To[768];              // 从MLX90640读取的温度数据
bool lock = false;
bool freeze = false;
unsigned long start_time;
bool probe_really = false;

enum probe_states {
    POWER_ON_DELAY,
    CONNECTION_FAILED,
    CONNECTED
};


paramsMLX90640 mlx90640;

// 判断是否连接`MLX90640`探头
boolean isConnected(){
    Wire.beginTransmission((uint8_t)MLX90640_address);
    if (Wire.endTransmission() != 0){return (false);}
    return (true);
}  

// 通过输入的0~180范围的数值，计算伪彩色，更新到全局变量`R_colour`，`G_colour`，`B_colour`中
void getColour(int j){
    if (j >= 0 && j < 30)
       {
        R_colour = 0;
        G_colour = 0;
        B_colour = 20 + 4 * j;
       }
    
    if (j >= 30 && j < 60)
       {
        R_colour = 4 * (j - 30);
        G_colour = 0;
        B_colour = 140 - 2 * (j - 30);
       }

    if (j >= 60 && j < 90)
       {
        R_colour = 120 + 4 * (j - 60);
        G_colour = 0;
        B_colour = 80 - 2 * (j - 60);
       }

    if (j >= 90 && j < 120)
       {
        R_colour = 255;
        G_colour = 0 + 2 * (j - 90);
        B_colour = 10 - (j - 90) / 3;
       }

    if (j >= 120 && j < 150)
       {
        R_colour = 255;
        G_colour = 60 + 175 * (j - 120) / 30;
        B_colour = 0;
       }

    if (j >= 150 && j <= 180)
       {
        R_colour = 255;
        G_colour = 235 + (j - 150) * 20 / 30;
        B_colour = 0 + 85 * (j - 150) / 10;
       }
}

void prob_power_open(){
   pinMode(MLX_VDD, OUTPUT);
   digitalWrite(MLX_VDD, LOW);
   #if defined(KALMAN)  // 初始化卡尔曼数组
   KalmanArrayInit();
   #endif
   Wire.setSDA(MLX_SDA);
   Wire.setSCL(MLX_SCL);  // 配置好I2C引脚
   start_time = millis();
}

void prob_power_off(){
   pinMode(MLX_VDD, OUTPUT);
   digitalWrite(MLX_VDD, HIGH);
}

int probeSetup(){
   static uint16_t count_retry = 0;
   // static unsigned long start_time = millis();
   while (millis() - start_time < MXL_STARTUP_DELAY){delay(10);}    // 等待一段时间后再启动探头
   
   while(!mlx_is_connected){   
      Wire.begin(); 
      delay(5);
      Wire.setClock(800000); //Increase I2C clock speed to 800kHz
      delay(5);
      mlx_is_connected = isConnected();
      count_retry++;
   }
   prob_status = PROB_INITIALIZING;
   if (mlx_is_connected == false){return CONNECTION_FAILED;}
   static uint16_t eeMLX90640[832]; 
   int state;
   state = MLX90640_DumpEE(MLX90640_address, eeMLX90640);
   state = MLX90640_ExtractParameters(eeMLX90640, &mlx90640);
   
   MLX90640_I2CWrite(0x33, 0x800D, 6401);    // writes the value 1901 (HEX) = 6401 (DEC) in the register at position 0x800D to enable reading out the temperatures!!!
   MLX90640_SetRefreshRate(MLX90640_address, 0x05); //Set rate to 8Hz effective - Works at 800kHz
   return CONNECTED;
}

inline bool probe_is_freezed(){
   if(!mlx_is_connected){return true;}
   if(!flag_sensor_ok){return true;}
   if(flag_in_photo_mode){return true;}
   else {return false;}
}


int sensor_loop(){
   if (!probe_is_freezed()){ // 如果画面被暂停会跳过这个热成像图的刷新
   
      while (pix_cp_lock == true) {delay(5);}
      prob_lock = true;
      for (byte x = 0 ; x < 2 ; x++){
         static uint16_t mlx90640Frame[834];
         static int status;
         static float vdd,Ta,tr,emissivity;

         status = MLX90640_GetFrameData(MLX90640_address, mlx90640Frame);
         vdd = MLX90640_GetVdd(mlx90640Frame, &mlx90640);

         Ta = MLX90640_GetTa(mlx90640Frame, &mlx90640);
         tr = Ta - TA_SHIFT; //Reflected temperature based on the sensor ambient temperature
         emissivity = 0.95;
         MLX90640_CalculateTo(mlx90640Frame, &mlx90640, emissivity, tr, mlx90640To);
      }

      // mlx90640To[0] = 0.33 * (mlx90640To[32] + mlx90640To[33] + mlx90640To[1]);    // eliminate the error-pixels
      T_min = mlx90640To[0];
      T_max = mlx90640To[0];
      T_avg = mlx90640To[0];
      for (int i = 1; i < 768; i++){
         if((mlx90640To[i] > -41) && (mlx90640To[i] < 301))
            {
               if(mlx90640To[i] < T_min)
                  {
                  T_min = mlx90640To[i];
                  }

               if(mlx90640To[i] > T_max)
                  {
                  T_max = mlx90640To[i];
                  max_x = i / 32;
                  max_y = i % 32;
                  }
            #if defined(KALMAN)
            if (flag_use_kalman==true){mlx90640To[i] = KalmanFilter(&kfpVar3Array[i], mlx90640To[i]);}
            #endif
            }
         else if(i > 0){
               mlx90640To[i] = mlx90640To[i-1];
            }
         else{
                mlx90640To[i] = mlx90640To[i+1];
            }
            T_avg = T_avg + mlx90640To[i];
         }
      T_avg = T_avg / 768;
      #if defined(KALMAN)
      if (flag_use_kalman==true){
         T_avg = KalmanFilter(&kfpVar1, T_avg);
         T_max = KalmanFilter(&kfpVar2, T_max);
         T_min = KalmanFilter(&kfpVar3, T_min);
      }
      #endif
      prob_lock = false;
   }
   return 0;
}



#endif